#include "BLLDevice.h"

#include <regex>

BLLDevice::BLLDevice(const char* portName, const char switchCtrl)
{
    std::lock_guard<std::mutex> lock(m_mutex);
    m_switchCtrl = switchCtrl;
    m_switchCtrl = m_switchCtrl & 0x00ff;
    m_isQuit = false;
    m_isInit = false;
    m_fmoduleCurrent = -1;
    m_portName = std::make_shared<std::string>(portName);
    m_devinfo = std::shared_ptr<char>(new char[256]);
    std::memset(m_devinfo.get(), 0, 256);
    m_thread = std::make_shared<std::thread>(&BLLDevice::threadLoop, this);
    m_errcnt = 0;

    m_ch1Status = -1;
    cmd_stop = true;
    cmd_start = false;
    cmd_moduleCurrent = -1.0f;
    m_temperature = 0x080;
    m_power = 0x1000;
}

BLLDevice::~BLLDevice()
{
    if (m_thread) {
        std::lock_guard<std::mutex> lock(m_mutex);
        m_isQuit = true;
    }
    if (m_thread) {
        m_thread->join();
    }
}

const char* BLLDevice::getDevInfo()
{
    const char* ret = nullptr;
    {
        std::lock_guard<std::mutex> lock(m_mutex);
        if (m_isInit && m_devinfo) {
            ret = m_devinfo.get();
        }
    }

    return ret;
}

bool BLLDevice::isHasError()
{
    bool other_err = m_isInit && m_ch1Status > -1 && (m_ch1Status & 0xC8);
    return m_errcnt >= 3 || other_err;
}

bool BLLDevice::isInitialized()
{
    return m_isInit;
}

int BLLDevice::getCH1Status()
{
    if (!m_isInit) {
        return -1;
    }
    return m_ch1Status;
}

bool BLLDevice::isStopped()
{
    return m_ch1Status > -1 && !(m_ch1Status & 0x20);
}

bool BLLDevice::isStarted()
{
    return m_ch1Status > -1 && (m_ch1Status & 0x20);
}

bool BLLDevice::isTemperatureStabilized()
{
    return m_ch1Status > -1 && (m_ch1Status & 0x04);
}

int BLLDevice::getTemperture()
{
    if (!m_isInit) {
        return 0x0080;
    } else {
        return m_temperature;
    }
}

int BLLDevice::getPower()
{
    if (!m_isInit) {
        return 0x01000;
    } else {
        return m_power;
    }
}

void BLLDevice::stop()
{
    std::lock_guard<std::mutex> lock(m_mutex);
    cmd_start = false;
    cmd_moduleCurrent = -1;
    cmd_stop = true;
}

void BLLDevice::start()
{
    std::lock_guard<std::mutex> lock(m_mutex);
    cmd_start = true;
}

void BLLDevice::setCurrent(float rate)
{
    // if (m_isInit && m_ch1Status > -1 && m_ch1Status & 0x20) {
    //     // SPDLOG_WARN("Do not set BLL OEM SLD module current after it started!");
    //     return;
    // }
    if (rate >= 0 && rate <= 50.0f) {
        if (rate > 1.0f) {
            SPDLOG_WARN("Set module current greater than factory parameter!");
        }
        std::lock_guard<std::mutex> lock(m_mutex);
        cmd_moduleCurrent = rate;
    }
}

void ms_sleep(int time_ms)
{
    std::this_thread::sleep_for(std::chrono::milliseconds(time_ms));
}

/**
 * @brief 发送光源通信命令并等待返回
 *
 * @param serialport 串口
 * @param cmd 命令, null结尾字符串
 * @param buf 命令回复的数据存放缓冲
 * @param buflen 存放缓冲的长度
 * @return true 成功
 * @return false 失败
 */
bool bll_tx_rx(
    std::shared_ptr<SerialPort> serialport,
    const char* cmd,
    char* buf, int buflen);

/**
 * @brief 解释接收到的数据为光源信息
 *
 * @param buf 解释接收到的数据
 * @return std::shared_ptr<std::string> 光源设备信息
 */
std::shared_ptr<std::string> parse_bll_info(const char* buf);

/**
 * @brief 解释接收到的数据为光源当前工作模式
 *
 * @param buf 解释接收到的数据
 * @return char L:本地控制模式 U:UART控制模式 其它:错误
 */
char parse_bll_cmode(const char* buf);

/**
 * @brief 解释接收到的数据为光源当前内部开关状态
 *
 * @param buf 解释接收到的数据
 * @return int 0~255: 开关状态, 其它: 错误
 */
int parse_bll_swstate(const char* buf);

/**
 * @brief 解释接收到的数据为光源参数值
 *
 * @param buf 解释接收到的数据
 * @return int 0x0000~0x0FFFF:有效数据值, 其它错误值
 */
int parse_bll_parameter(const char* buf);

/**
 * @brief 解释接收到的数据为光源开关状态
 *
 * @param buf 解释接收到的数据
 * @return int 0x0000~0x0FF:有效数据值, 其它错误值
 */
int parse_bll_onoffswitch(const char* buf);

/**
 * @brief 解释接收到的数据为光源当前温度和功率
 *
 * @param buf 解释接收到的数据
 * @param temp 返回温度结果, -127 ~ 127摄氏度为正常值, 其它值为异常
 * @param power 返回功率结果, 0 ~ 4095mW为正常值, 其它为异常值
 */
void parse_bll_temperatureAndPower(const char* buf, int* temp, int* power);

void BLLDevice::threadLoop()
{
    constexpr auto buflen = 1024;
    auto cmdbuf = std::shared_ptr<char>(new char[buflen]);
    auto rxbuf = std::shared_ptr<char>(new char[buflen]);
    auto serialport = std::make_shared<SerialPort>();

    serialport->setPortName(this->m_portName->data());
    serialport->setBaudRate(57600);
    serialport->setDataBits(SerialPort::Data8);
    serialport->setStopBits(SerialPort::OneStop);
    serialport->setParity(SerialPort::NoParity);
    serialport->setFlowControl(SerialPort::NoFlowControl);
    serialport->setTimeoutParams(30, 0, 0, 0, 0);

    auto checkSerialPort = [&serialport]() -> bool {
        if (!serialport)
            return false;
        if (!serialport->isOpen()) {
            serialport->open();
            if (serialport->isOpen()) {
                serialport->flush();
                serialport->waitForReadyRead(100);
                serialport->clear();
            }
        }
        return serialport->isOpen();
    };

    auto onePDU = [this, serialport](const char* cmd,
                      char* buf, int buflen) -> bool {
        auto ret = bll_tx_rx(serialport, cmd, buf, buflen);
        if (!ret && this->m_errcnt < 3) {
            this->m_errcnt++;
            SPDLOG_WARN("No Acknowledge receive from BLL SLD Device!");
        }
        return ret;
    };

    while (true) {
        if (m_mutex.try_lock()) {
            if (m_isQuit) {
                m_mutex.unlock();
                break;
            }
            m_mutex.unlock();
        }

        if (this->m_errcnt > 5 && serialport->isOpen()) {
            serialport->close();
            this->m_isInit = false;
        }

        if (!checkSerialPort()) {
            ms_sleep(100);
            continue;
        }

        if (!this->m_isInit) {
            //  传送控制命令
            bool ok = true;
            while (!this->m_isInit) {
                // 初始化
                // 读序列号
                ok = ok && onePDU("I\r\n", rxbuf.get(), buflen);
                if (!ok)
                    break;
                auto ptr = parse_bll_info(rxbuf.get());
                if (ptr && !ptr->empty()) {
                    std::memcpy(m_devinfo.get(), ptr->c_str(), ptr->length());
                    m_devinfo.get()[ptr->length()] = 0;
                } else {
                    break;
                }

                // 设置为UART远程模式
                ok = ok && onePDU("MU\r\n", rxbuf.get(), buflen);
                if (!ok)
                    break;
                ok = ok && parse_bll_cmode(rxbuf.get()) == 'U';
                if (!ok)
                    break;

                // 确保设备处于停止状态
                ok = ok && onePDU("UC?\r\n", rxbuf.get(), buflen);
                if (!ok)
                    break;
                {
                    auto status = parse_bll_onoffswitch(rxbuf.get());
                    if (status >= 0 && status & 0x20) {
                        ok = ok && onePDU("UC1\r\n", rxbuf.get(), buflen);
                        if (!ok)
                            break;
                        status = parse_bll_onoffswitch(rxbuf.get());
                        if (status >= 0)
                            m_ch1Status = status;
                        if (status & 0x20)
                            break;
                    }
                }

                // 读取内部开关状态
                ok = ok && onePDU("US?\r\n", rxbuf.get(), buflen);
                if (!ok)
                    break;
                {
                    int sw = parse_bll_swstate(rxbuf.get());
                    ok = ok && sw != -1;
                    if (!ok)
                        break;
                    int req_sw = m_switchCtrl;
                    req_sw ^= sw;
                    if (0x01 & req_sw) {
                        onePDU("US1\r\n", rxbuf.get(), buflen);
                    }
                    if (0x10 & req_sw) {
                        onePDU("US5\r\n", rxbuf.get(), buflen);
                    }
                    if (0x20 & req_sw) {
                        onePDU("US6\r\n", rxbuf.get(), buflen);
                    }
                    if (0x40 & req_sw) {
                        onePDU("US7\r\n", rxbuf.get(), buflen);
                    }
                    if (0x80 & req_sw) {
                        onePDU("US8\r\n", rxbuf.get(), buflen);
                    }
                    ok = ok && onePDU("US?\r\n", rxbuf.get(), buflen);
                    if (!ok)
                        break;
                    sw = parse_bll_swstate(rxbuf.get());
                    ok = ok && sw != -1;
                    if (!ok)
                        break;
                    req_sw = m_switchCtrl;
                    req_sw ^= sw;
                    ok = ok && ((req_sw & 0x00f1) == 0);
                    if (!ok) {
                        SPDLOG_ERROR("BLL SLD can not set interal switch to '{:08b}'", m_switchCtrl);
                        break;
                    }
                    if (m_switchCtrl != 1) {
                        SPDLOG_WARN("BLL SLD interal switch was set to '{:08b}'", m_switchCtrl);
                    }
                }

                // 读取出厂默认电流值
                ok = ok && onePDU("UF14\r\n", rxbuf.get(), buflen);
                if (!ok)
                    break;
                {
                    int res = parse_bll_parameter(rxbuf.get());
                    ok = ok && res >= 0;
                    if (!ok)
                        break;
                    m_fmoduleCurrent = res;
                }

                //  确认初始化
                if (ok) {
                    std::lock_guard<std::mutex> lock(m_mutex);
                    this->m_isInit = true;
                    this->m_errcnt = 0;
                }
                break;
            }

            continue;
        }

        // 执行命令
        if (cmd_stop) {
            // 停止光源输出
            if (!onePDU("UC?\r\n", rxbuf.get(), buflen)) {
                continue;
            }
            auto status = parse_bll_onoffswitch(rxbuf.get());
            m_ch1Status = status;
            if (status < 0 && m_errcnt < 3)
                m_errcnt++;
            if (status >= 0 && status & 0x20) {
                if (!onePDU("UC1\r\n", rxbuf.get(), buflen)) {
                    continue;
                }
                status = parse_bll_onoffswitch(rxbuf.get());
                m_ch1Status = status;
                if (status < 0 && m_errcnt < 3)
                    m_errcnt++;
                else if (status >= 0 && status & 0x20) {
                    continue;
                }
            }
            {
                std::lock_guard<std::mutex> lock(m_mutex);
                cmd_stop = false;
            }
            continue;
        }
        if (cmd_moduleCurrent >= 0 && cmd_moduleCurrent <= 50.0f) {
            // 设置光学模块电流
            unsigned int t = static_cast<unsigned int>(cmd_moduleCurrent * m_fmoduleCurrent);
            t = t & 0x0FFFFu;
            auto cmd = fmt::format("UJ14{:04X}\r\n", t);
            if (onePDU(cmd.c_str(), rxbuf.get(), buflen)) {
                int v = parse_bll_parameter(rxbuf.get());
                if (v == t) {
                    std::lock_guard<std::mutex> lock(m_mutex);
                    cmd_moduleCurrent = -1;
                }
            }
            continue;
        }
        if (cmd_start) {
            // 启动光源输出
            if (!onePDU("UC?\r\n", rxbuf.get(), buflen)) {
                continue;
            }
            auto status = parse_bll_onoffswitch(rxbuf.get());
            m_ch1Status = status;
            if (status < 0 && m_errcnt < 3)
                m_errcnt++;
            if (status >= 0 && !(status & 0x20)) {
                if (!onePDU("UC1\r\n", rxbuf.get(), buflen)) {
                    continue;
                }
                status = parse_bll_onoffswitch(rxbuf.get());
                m_ch1Status = status;
                if (status < 0 && m_errcnt < 3)
                    m_errcnt++;
                else if (status >= 0 && !(status & 0x20)) {
                    m_errcnt = 0;
                    continue;
                }
            }
            {
                std::lock_guard<std::mutex> lock(m_mutex);
                cmd_start = false;
            }
            continue;
        }

        //  读取设备运行状态
        if (!onePDU("UC?\r\n", rxbuf.get(), buflen)) {
            continue;
        } else {
            m_ch1Status = parse_bll_onoffswitch(rxbuf.get());
            if (m_ch1Status < 0) {
                if (m_errcnt < 3)
                    m_errcnt++;
            } else {
                m_errcnt = 0;
            }
        }
        //  读取设备温度和功率
        if (!onePDU("UT\r\n", rxbuf.get(), buflen)) {
            continue;
        } else {
            int temperature, power;
            parse_bll_temperatureAndPower(rxbuf.get(), &temperature, &power);
            if (temperature >= -127 && temperature <= 127) {
                m_temperature = temperature;
                m_errcnt = 0;
            } else {
                if (m_errcnt < 3)
                    m_errcnt++;
            }
            if (power >= 0 && power < 0x01000) {
                m_power = power;
                m_errcnt = 0;
            } else {
                if (m_errcnt < 3)
                    m_errcnt++;
            }
        }
        ms_sleep(100);
    }

    // if (!serialport->isOpen()) {
    //     bll_tx_rx(serialport, "ML\r\n", rxbuf.get(), buflen);
    // }
}

bool bll_tx_rx(
    std::shared_ptr<SerialPort> serialport,
    const char* cmd,
    char* buf, int buflen)
{
    if (!serialport || !buf || buflen < 64)
        return false;

    int rx_cnt = 0;
    int rx_end = 0;
    auto cmdlen = strlen(cmd);
    if (cmdlen < 2 || cmdlen > 32) {
        return false;
    }

    buflen--;
    buf[0] = 0;
    for (int i = 0; i < 3; i++) {
        rx_cnt = 0;
        serialport->write(cmd, static_cast<int>(cmdlen));
        serialport->flush();
        for (int j = 0; j < 3; i++) {
            if (!serialport->waitForReadyRead(50)) {
                break;
            }
            int cnt = serialport->read(&(buf[rx_cnt]), buflen - rx_cnt);
            if (cnt > 0) {
                rx_cnt += cnt;
                if (rx_cnt > 1 && buf[rx_cnt - 1] == '\n' && buf[rx_cnt - 2] == '\r') {
                    rx_end = 1;
                }
                if (rx_end > 0) {
                    buf[rx_cnt] = 0;
                    break;
                }
            } else {
                break;
            }
        }
        if (rx_end > 0)
            break;
    }

    buf[buflen] = 0;
    if (rx_end != 1) {
        SPDLOG_WARN("Receive invalid data : {}", std::vector<char>(buf, buf + rx_cnt));
    }
    return rx_end == 1;
}

std::shared_ptr<std::string> parse_bll_info(const char* buf)
{
    std::regex regex(R"(^I:(.*)\r\n)");
    std::cmatch match;
    if (std::regex_search(buf, match, regex) && match.size() > 1) {
        return std::make_shared<std::string>(match[1].str());
    }
    return nullptr;
}

char parse_bll_cmode(const char* buf)
{
    std::regex regex(R"(^M([A-Z]{1})\r\n)");
    std::cmatch match;
    if (std::regex_search(buf, match, regex) && match.size() > 1) {
        return match[1].str()[0];
    }
    return 'E';
}

int parse_bll_swstate(const char* buf)
{
    std::regex regex(R"(^US([0-9]{1,2})\r\n)");
    std::cmatch match;
    if (std::regex_search(buf, match, regex) && match.size() > 1) {
        return std::stoi(match[1].str(), nullptr, 16);
    }
    return -1;
}

int parse_bll_parameter(const char* buf)
{
    std::regex regex(R"(^U[PFJ]1[0-9]([0-9A-F]{1,8})\r\n)");
    std::cmatch match;
    if (std::regex_search(buf, match, regex) && match.size() > 1) {
        return static_cast<int>(std::stoul(match[1].str(), nullptr, 16));
    }
    return -1;
}

int parse_bll_onoffswitch(const char* buf)
{
    std::regex regex(R"(^UC1([0-9A-F]{1,2})\r\n)");
    std::cmatch match;
    if (std::regex_search(buf, match, regex) && match.size() > 1) {
        return static_cast<int>(std::stoul(match[1].str(), nullptr, 16));
    }
    return -1;
}

void parse_bll_temperatureAndPower(const char* buf, int* temp, int* power)
{
    *temp = 0x0080;
    *power = 0x1000;
    std::regex regex(R"(^UT([0-9A-F]{2})([0-9A-F]{3})\r\n)");
    std::cmatch match;
    if (std::regex_search(buf, match, regex) && match.size() > 2) {
        *temp = static_cast<int>(std::stoul(match[1].str(), nullptr, 16));
        *power = static_cast<int>(std::stoul(match[2].str(), nullptr, 16));
    }
}
